// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KinectObjectDetectorContext.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Vision.ObjectDetection
{
    using System;
    using System.Collections.Generic;
    using System.IO;
    using System.Windows;
    using Microsoft.Robotics.FiniteStateMachine;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Tracing;
    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.Cameras;

    /// <summary>
    /// FSM context for kinect IR plus depth synchronizer
    /// </summary>
    public class KinectObjectDetectorContext : IContext<KinectObjectDetectorContext, KinectObjectDetectorContext>, IDisposable
    {
        /// <summary>
        /// Object pose predictors
        /// </summary>
        private readonly ObjectPosePredictor[] objectPosePredictors;

        /// <summary>
        /// The most recently acquired IR frame
        /// </summary>
        private ImageFrame<ushort> latestIRframe;

        /// <summary>
        /// The most recently acquired depth frame
        /// </summary>
        private ImageFrameDepth latestDepthFrame;

        /// <summary>
        /// Kinect camera calibration
        /// </summary>
        private KinectCameraCalibration cameraCalibration;

        /// <summary>
        /// Object detector configurations
        /// </summary>
        private KinectIRObjectDetectorConfiguration[] objectDetectorConfigurations;

        /// <summary>
        /// Tracks whether Dispose has been called.
        /// </summary>
        private bool disposed = false;

        /// <summary>
        /// Initializes a new instance of the <see cref="KinectObjectDetectorContext" /> class
        /// </summary>
        /// <param name="detectorConfigurationFiles">Object detector configuration files</param>
        /// <param name="cameraPose">Extrinsic camera pose</param>
        /// <param name="intrinsicCameraMatrix">Intrinsic camera matrix</param>
        public KinectObjectDetectorContext(
            string[] detectorConfigurationFiles,
            Pose cameraPose,
            Matrix3 intrinsicCameraMatrix)
        {
            this.cameraCalibration = new KinectCameraCalibration(intrinsicCameraMatrix, cameraPose);

            this.objectDetectorConfigurations = new KinectIRObjectDetectorConfiguration[detectorConfigurationFiles.Length];
            this.objectPosePredictors = new ObjectPosePredictor[detectorConfigurationFiles.Length];

            for (int i = 0; i < detectorConfigurationFiles.Length; ++i)
            {
                this.objectDetectorConfigurations[i] = KinectIRObjectDetectorConfiguration.Create(detectorConfigurationFiles[i]);
                this.objectPosePredictors[i] = new ObjectPosePredictor(this.objectDetectorConfigurations[i]);
            }

            this.IsReadyForImages = true;
            this.LastUsedId = string.Empty;
        }

        /// <summary>
        /// Gets or sets the delegate for posting detection results
        /// </summary>
        public Action PostDetectionResult { get; set; }

        /// <summary>
        /// Gets results of object detection
        /// </summary>
        public ObjectPoseEstimate[] DetectionResults { get; private set; }

        /// <summary>
        /// Gets the IR frame used in the last detection
        /// </summary>
        public ImageFrame<ushort> LastUsedIRframe { get; private set; }

        /// <summary>
        /// Gets the depth frame used in the last detection
        /// </summary>
        public ImageFrameDepth LastUsedDepthFrame { get; private set; }

        /// <summary>
        /// Gets the saved originating time of the frames used in last detection
        /// </summary>
        public long LastUsedFrameOriginatingTime { get; private set; }

        /// <summary>
        /// Gets or sets a value indicating whether ready to acquire kinect images
        /// </summary>
        public bool IsReadyForImages { get; set; }

        /// <summary>
        /// Gets or sets inputs to the FSM from the outside world.
        /// </summary>
        public KinectObjectDetectorContext Input { get; set; }

        /// <summary>
        /// Gets or sets output from the FSM to the outside world.
        /// </summary>
        public KinectObjectDetectorContext Output { get; set; }

        /// <summary>
        /// Gets the originating time of last image frames received
        /// </summary>
        public long LatestFrameOriginatingTime { get; private set; }

        /// <summary>
        /// Gets or sets the last time a detection message was posted
        /// </summary>
        public long LastDetectionPostTime { get; set; }

        /// <summary>
        /// Gets Last set of detection results
        /// </summary>
        protected List<PatchCluster> LastUsedResults { get; private set; }

        /// <summary>
        /// Gets or sets the last used result Id
        /// </summary>
        protected string LastUsedId { get; set; }

        /// <summary>
        /// Save the latest acquired Depth frame
        /// </summary>
        /// <param name="depthImage">Latest depth image</param>
        /// <param name="irFrame">Latest IR frame</param>
        /// <param name="originTime">Originating time of Depth image message</param>
        public void SaveImageFrames(ImageFrameDepth depthImage, ImageFrame<ushort> irFrame, long originTime)
        {
            this.latestDepthFrame = depthImage;
            this.latestIRframe = irFrame;
            this.LatestFrameOriginatingTime = originTime;
        }

        /// <summary>
        /// Reset the context before starting a new acquisition round
        /// </summary>
        public void Reset()
        {
            this.LatestFrameOriginatingTime = 0;
            this.LastUsedDepthFrame = null;
            this.LastUsedIRframe = null;
            this.DetectionResults = null;
        }

        /// <summary>
        /// Runs object detection using latest depth and IR images
        /// </summary>
        public void RunObjectDetection()
        {
            // This is an efficiency shortcut - it assumes all configurations use the same
            // no reading value. It is a reasonable assumption
            this.ReplaceNoMeasurementValues(this.objectDetectorConfigurations[0], this.latestDepthFrame);

            this.LastUsedIRframe = this.latestIRframe;
            this.LastUsedFrameOriginatingTime = this.LatestFrameOriginatingTime;
            this.LastUsedIRframe = this.latestIRframe;
            this.LastUsedDepthFrame = this.latestDepthFrame;

            ObjectDetectionFeaturization featurizer = new ObjectDetectionFeaturization(
                                this.latestIRframe,
                                this.latestDepthFrame,
                                this.cameraCalibration.FocalX,
                                this.cameraCalibration.FocalY);

            List<ObjectPoseEstimate> poses = new List<ObjectPoseEstimate>();

            foreach (ObjectPosePredictor predictor in this.objectPosePredictors)
            {
                List<PatchCluster> results = predictor.Predict(featurizer, this.cameraCalibration);

                this.LastUsedResults = results;
                List<Shape2DCluster> clusters = Shape2DCluster.Cluster(results) as List<Shape2DCluster>;

                foreach (Shape2DCluster cluster in clusters)
                {
                    ((List<IShape2D>)cluster.Components).Sort((a, b) => { return -a.Confidence.CompareTo(b.Confidence); });
                    PatchCluster r = cluster.Components[0] as PatchCluster;
                    ObjectPoseEstimate poseEstimate = r.PoseEstimate;
                    Vector3? globalPos = this.cameraCalibration.ImageToWorldConvert(poseEstimate.CameraPixelPose.Position);
                    if (true == globalPos.HasValue)
                    {
                        Pose global = new Pose(globalPos.Value, poseEstimate.CameraPixelPose.Orientation);
                        ObjectPoseEstimate poseEstimateFinal = new ObjectPoseEstimate(r.PoseEstimate, global);
                        poses.Add(poseEstimateFinal);
                    }
                    else
                    {
                        Vector3 p = r.PoseEstimate.CameraPixelPose.Position;
                        TraceOut.Warning(TraceContexts.Vision, "No global position for Camera Position {0} {1} {2}", p.X, p.Y, p.Z);
                    }
                }
            }

            poses.Sort((p, q) => { return -(p.Confidence - p.ConfidenceThreshold).CompareTo((q.Confidence - q.ConfidenceThreshold)); });
            this.DetectionResults = poses.ToArray();
        }

        /// <summary>
        /// Implement IDisposable
        /// </summary>
        public void Dispose()
        {
            this.Dispose(true);
            GC.SuppressFinalize(this);
        }

        /// <summary>
        /// Dispose unmanaged resources
        /// </summary>
        /// <param name="disposing">True if dispose is called by user code, false otherwise</param>
        protected virtual void Dispose(bool disposing)
        {
            // Check to see if Dispose has already been called. 
            if (false == this.disposed)
            {
                if (disposing)
                {
                    if (null != this.objectPosePredictors)
                    {
                        for (int i = 0; i < this.objectPosePredictors.Length; ++i)
                        {
                            if (null != this.objectPosePredictors[i])
                            {
                                this.objectPosePredictors[i].Dispose();
                                this.objectPosePredictors[i] = null;
                            }
                        }
                    }
                }
            }
        }

        /// <summary>
        /// replace no measurement values in a depth frame
        /// </summary>
        /// <param name="detectorConfig">Detector configuration</param>
        /// <param name="depthFrame">Depth frame</param>
        private void ReplaceNoMeasurementValues(KinectIRObjectDetectorConfiguration detectorConfig, ImageFrameDepth depthFrame)
        {
            for (int i = 0; i < depthFrame.ImageData.Length; ++i)
            {
                if (depthFrame.ImageData[i] == 0)
                {
                    depthFrame.ImageData[i] = detectorConfig.NoReadingDepthValue;
                }
            }
        }
    }
}
